
#include "PreHeader.h"
#include "cCatmullRomSpline.h"

#define M11  0.0f
#define M12  1.0f
#define M13  0.0f
#define M14  0.0f
#define M21 -0.5f
#define M22  0.0f
#define M23  0.5f
#define M24  0.0f
#define M31  1.0f
#define M32 -2.5f
#define M33  2.0f
#define M34 -0.5f
#define M41 -0.5f
#define M42  1.5f
#define M43 -1.5f
#define M44  0.5f

b2Vec2 cCatmullRomSpline::Interpolate(tFloat time, const tPointArray& pointArray)
{
	tInt size = (tInt)pointArray.size();
	if (size < 4)
	{
		return pointArray[0];
	}

	tInt divide = size - 1;
	tFloat timeDivide = 1.0f / tFloat(divide);
	tInt index = tInt(time / timeDivide);
	tFloat t = (time-(timeDivide*index))/timeDivide;

	b2Vec2 point;

	if (index == 0)
	{
		point = Interpolate(t, pointArray[0], pointArray[0], pointArray[1], pointArray[2]);
	}
	else if (index == divide - 1)
	{
		point = Interpolate(t, pointArray[index-1], pointArray[index], pointArray[index+1], pointArray[index+1]);
	}
	else if (index == divide)
	{
		point = pointArray[index];
	}
	else 
	{
		point = Interpolate(t, pointArray[index-1], pointArray[index], pointArray[index+1], pointArray[index+2]);
	}

	return point;
}

b2Vec2 cCatmullRomSpline::Interpolate(tFloat time, const b2Vec2& p1, const b2Vec2& p2, const b2Vec2& p3, const b2Vec2& p4)
{
	tFloat c1, c2, c3, c4;
	b2Vec2 point;

	c1 = M12*p2.x;   
	c2 = M21*p1.x + M23*p3.x;   
	c3 = M31*p1.x + M32*p2.x + M33*p3.x + M34*p4.x;   
	c4 = M41*p1.x + M42*p2.x + M43*p3.x + M44*p4.x;
	point.x = (((c4*time + c3)*time +c2)*time + c1);

	c1 = M12*p2.y;
	c2 = M21*p1.y + M23*p3.y;   
	c3 = M31*p1.y + M32*p2.y + M33*p3.y + M34*p4.y;   
	c4 = M41*p1.y + M42*p2.y + M43*p3.y + M44*p4.y;   
	point.y = (((c4*time + c3)*time +c2)*time + c1);

	return point;
}